#include <iostream>
#include <stdio.h>
#include <unistd.h>

#include "can_ctrl.h"
#include "controlcan.h"
#include "leg_ctrl.h"

using namespace std;


/* 接收线程 */
void *can_receive_thread(void* param)
{
    extern Leg_Ctrl *g_leg_ctrl;
    uint32_t reclen = 0;
    uint8_t motor_id;
    Can_Ctrl *can_ctrl = (Can_Ctrl*)param;

    while(1 == can_ctrl->can_thread_getflag()) {
        if((reclen = VCI_Receive(VCI_USBCAN2, 0, 0, can_ctrl->recv, 3000, 100))>0) {
        #if 0
            for(uint32_t i = 0; i < reclen; i++) {
                cout << "CAN1";
                printf(" RX_ID:0x%x",can_ctrl->recv[i].ID);
                if(can_ctrl->recv[i].ExternFlag==0)  { // 帧格式：标准帧 or 扩展帧
                    cout <<" Standard ";
                } else {
                    cout <<" Extend ";
                }

                if(can_ctrl->recv[i].RemoteFlag==0) {  // 帧类型：数据帧 or 远程帧
                    cout <<" Data ";   
                } else  {
                    cout << " Remote ";  
                }

                cout << " DLC:" << can_ctrl->recv[i].DataLen;   //帧长度
                printf(" data:0x");	                            //数据
                for(uint8_t j = 0; j < can_ctrl->recv[i].DataLen; j++) {
                    printf(" %02X", can_ctrl->recv[i].Data[j]);
                }
                printf(" TimeStamp:0x%08X\n",can_ctrl->recv[i].TimeStamp);  //时间标识
            }
        #else
            for(uint32_t i = 0; i < reclen; i++) {
                if(can_ctrl->recv[i].RemoteFlag == 0) { // 帧类型：数据帧
                    if(can_ctrl->recv[i].ExternFlag == 1) {   //标准帧为YBT, 扩展帧为AK
                        for(uint8_t num = 0; num < g_leg_ctrl->p_motor_ctrl->get_motor_num(); num++) {
                            motor_id = g_leg_ctrl->p_motor_ctrl->get_motor_item(num)->get_motor_id();
                            if(((can_ctrl->recv[i].ID & 0xff) == motor_id) && \
                                (g_leg_ctrl->p_motor_ctrl->get_motor_item(num)->get_motor_type() == MOTOR_TYPE_AK)) {
                                    g_leg_ctrl->p_motor_ctrl->get_motor_item(num)->set_cur_pos(can_ctrl->recv[i].Data);
                                    printf("AK:%d, cur_pos:%f\n", motor_id, g_leg_ctrl->p_motor_ctrl->get_motor_item(num)->get_cur_pos());
                                }
                        }
                    } else {                                    
                        for(uint8_t num = 0; num < g_leg_ctrl->p_motor_ctrl->get_motor_num(); num++) {
                            motor_id = g_leg_ctrl->p_motor_ctrl->get_motor_item(num)->get_motor_id();
                        #if 0
                            printf("can_recv:");	                            //数据
                            for(uint8_t j = 0; j < can_ctrl->recv[i].DataLen; j++) {
                                printf(" %02X", can_ctrl->recv[i].Data[j]);
                            }
                            printf("\n");
                        #endif
                            if((can_ctrl->recv[i].Data[0] == motor_id) && \
                                (g_leg_ctrl->p_motor_ctrl->get_motor_item(num)->get_motor_type() == MOTOR_TYPE_YBT)) {
                                    g_leg_ctrl->p_motor_ctrl->get_motor_item(num)->set_cur_pos(&can_ctrl->recv[i].Data[1]);         
                                    printf("YBT:%d, cur_pos:%f\n", motor_id, g_leg_ctrl->p_motor_ctrl->get_motor_item(num)->get_cur_pos());
                                }
                        }
                    }
                }
            }

        #endif
        }
        
    }
    printf("can thread exit\n");//退出接收线程
    return NULL;
}


int Can_Ctrl::send_func(CAN_RxHeaderTypeDef *pHeader) 
{
    if(pHeader->IDE == CAN_ID_STD) {
        send[0].ExternFlag = 0;
        send[0].ID = pHeader->StdId; 
    } else {
        send[0].ExternFlag = 1; 
        send[0].ID = pHeader->ExtId; 
    }  
    if(pHeader->RTR == CAN_RTR_DATA) {
        send[0].RemoteFlag = 0;  
    } else {
        send[0].RemoteFlag = 1; 
    }
    send[0].SendType = 0;
    send[0].DataLen = pHeader->DLC;
    for(uint8_t i = 0; i < pHeader->DLC; i++) {
        send[0].Data[i] = pHeader->Data[i];
    }

    if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1) {
        return 0;
    } else {
        cout << "device id:" << send[0].ID << "may not exist, or usb_can is offline!" << endl;
        return -1;
    }
}

int Can_Ctrl::can_ctrl_init()
{
    int num = VCI_FindUsbDevice2(pInfo);
    cout << ">>USBCAN DEVICE NUM:" << num << " PCS" << endl;

    for(int i = 0; i < num; i++){
        cout << "Device:" << i << endl;
        cout << ">>Get VCI_ReadBoardInfo success!" << endl;   
        cout << ">>Serial_Num:" << pInfo[i].str_Serial_Num << endl;
        cout << ">>hw_Type:" << pInfo[i].str_hw_Type << endl;
        cout << ">>Firmware Version:V";
        printf("%x.%x%x\n", (pInfo[i].fw_Version&0xF00)>>8, (pInfo[i].fw_Version&0xF0)>>4, pInfo[i].fw_Version&0x0F);
    }

    cout << ">>" << endl;
    cout << ">>" << endl;
    cout << ">>" << endl;

    if(VCI_OpenDevice(VCI_USBCAN2, 0, 0) == 1) {   //打开通道0
        cout << ">>open deivce success!" << endl;
    } else {
        cout << ">>open deivce error!" << endl;
        return -1;
    }

    //初始化参数，严格参数二次开发函数库说明书。
    VCI_INIT_CONFIG config;
    config.AccCode  = 0;
    config.AccMask  = 0xFFFFFFFF;
    config.Filter   = 1;        //接收所有帧
    config.Timing0  = 0x00;    /*1M bps  0x00  0x14*/
    config.Timing1  = 0x14;
    config.Mode     = 0;          //正常模式		
    
    if(VCI_InitCAN(VCI_USBCAN2, 0, 0, &config) != 1){
        cout << ">>Init CAN1 error!" << endl;
        VCI_CloseDevice(VCI_USBCAN2,0);
        return -1;
    }

    if(VCI_StartCAN(VCI_USBCAN2,0,0) != 1)
    {
        cout << ">>Start CAN1 error!" << endl;
        VCI_CloseDevice(VCI_USBCAN2,0);
        return -1;

    }
    //创建接收线程
    this->can_thread_enable();
    pthread_create(&pid, NULL, can_receive_thread, this);
    this->ready = 1;

    return 0;
}


int Can_Ctrl::can_ctrl_deinit()
{
    this->can_thread_disable();         //线程关闭指令
    pthread_join(pid, NULL);    //等待线程关闭

    usleep(100000);
    if(VCI_ResetCAN(VCI_USBCAN2, 0, 0) == 1) {       //复位CAN1通道
        cout << ">>reset deivce success!" << endl;
    } else {
        cout << ">>reset deivce error!" << endl;
        return -1;
    }

    usleep(100000);
    if(VCI_CloseDevice(VCI_USBCAN2,0) == 1) {   //关闭设备
        cout << ">>close deivce success!" << endl;
    } else {
        cout << ">>close deivce error!" << endl;
        return -1;
    }

    this->ready = 0;

    return 0;
}

